#include <Arduino.h>
// #include <string.h>
#include "fpga_reciver.h"
#include "udp.h"
ModbusRTU mb;
ModbusIP mb_tcp;  //ModbusIP object

const char startMarker = 'A';
const char endMarker = 'B';
const int maxDataSize = 14;
short H_data = 10;
short L_data = 10;

short cos_data = 0 ;
short sin_data = 0 ;
short pre_cos_data = 0 ;
short pre_sin_data = 0 ;



short status = 0;    //前两位为校正状态0x00代表空，0x01代表三差校正，0x10代表零位校正。后两位为零位状态,只有0x00和0x01
int received_length_Data;
int received_cos_Data;
int received_sin_Data;
static unsigned long previousMillis = 0;
static bool bitCleared = true;
byte receivedBytes[maxDataSize];

int dataIndex = 0;
bool newData = false;

void Serial_callback() {
  while (Serial1.available() > 0) 
  {
    char receivedChar = Serial1.read();

    if (receivedChar == startMarker) {
      dataIndex = 0;
      newData = true;
    } 
    else if (newData)
    {
      if (dataIndex < maxDataSize) 
      {
        receivedBytes[dataIndex] = receivedChar;
        dataIndex++;
      } 
      else 
      {
        // 解析完成
        if (dataIndex == maxDataSize && receivedChar == endMarker) 
        {
          //received_length_Data = (receivedBytes[0] << 24) |(receivedBytes[1] << 16) | (receivedBytes[2] << 8) | receivedBytes[3];
          //received_cos_Data = (receivedBytes[4] << 10) |(receivedBytes[5] << 2) ;
          //received_sin_Data  = (receivedBytes[5] << 10) |(receivedBytes[6] << 2) ;
          H_data = (receivedBytes[0] << 8) | receivedBytes[1];
          L_data = (receivedBytes[2] << 8) | receivedBytes[3];
          cos_data = (receivedBytes[4] << 8) |(receivedBytes[5] ) ;
          sin_data = (receivedBytes[6] << 8) |(receivedBytes[7] ) ;
          status = (receivedBytes[8] << 8) | (receivedBytes[9]);
          pre_cos_data = (receivedBytes[10] << 8) |(receivedBytes[11] ) ;
          pre_sin_data = (receivedBytes[12] << 8) |(receivedBytes[13] ) ;
          // status = (receivedBytes[8] << 8) |(receivedBytes[9] ) ;




          // 当经过零位时，将电源灯与其同步，并锁存1秒状态，然后清除
            // Check if the least significant bit is 1
          // if (status & 0x01 && bitCleared) {
          //     // Set the second least significant bit to 1
          //     status |= 0x02;

          //     // Record the current time
          //     previousMillis = millis();

          //     // Mark that the bit is not yet cleared
          //     bitCleared = false;
          //   }
          //   // Check if 1 second has passed and the bit is not cleared
          //   if (!bitCleared && millis() - previousMillis < 1000) {
          //     // Clear both the least significant bit and the second least significant bit
          //     status |= 0x03;

          //     // Mark that the bit has been cleared
          //     // bitCleared = true;
          //   }

          //   // Check if 1 second has passed and the bit is not cleared
          //   if (!bitCleared && millis() - previousMillis >= 1000) {
          //     // Clear both the least significant bit and the second least significant bit
          //     status &= ~0x03;

          //     // Mark that the bit has been cleared
          //     bitCleared = true;
          //   }




          // // 同步校准时信号灯的状态  
          // if (((status >> 8) & 0x03) == 0x02) {
          //   status = (status & 0xFFE3) | (0x02 << 2); // Clear bits 5, 4, 3 and set them to 010
          // }
          // else if ( ((status >> 8) & 0x03) == 0x01) {
          //   status = (status & 0xFFE3) | (0x01 << 2); // Clear bits 5, 4, 3 and set them to 010
          // }          


          //cos_data = (short)(receivedBytes[4] << 10) |(receivedBytes[5] << 2) / 4;
          //sin_data = (short)(receivedBytes[6] << 10) |(receivedBytes[7] << 2) / 4;

          if (receivedBytes[4] & 0x20) {
            cos_data = cos_data-16384; // Apply the sign if the sign bit is set
          }

          if (receivedBytes[6] & 0x20) {
            sin_data = sin_data-16384; // Apply the sign if the sign bit is set
          }

          if (receivedBytes[10] & 0x20) {
            pre_cos_data = pre_cos_data-16384; // Apply the sign if the sign bit is set
          }

          if (receivedBytes[12] & 0x20) {
            pre_sin_data = pre_sin_data-16384; // Apply the sign if the sign bit is set
          }

          mb.Hreg(REGN10, H_data);
          mb.Hreg(REGN11, L_data);
          mb.Hreg(REGN12, cos_data);
          mb.Hreg(REGN13, sin_data);
          mb.Hreg(REGN15, status);
          mb.Hreg(REGN17, pre_cos_data);
          mb.Hreg(REGN18, pre_sin_data);

        }
        newData = false;
      }
    }
  }
}



void fpga_reciver_init() {
  // HardwareSerial(Serial1,921600,);
  // Serial1.begin(921600,SERIAL_8N1,12,13);
  Serial1.begin(921600,SERIAL_8N1,12,13);
  // Serial1.begin(921600);
  Serial1.onReceive(Serial_callback);//设置回调函数
  // #if defined(ESP32) || defined(ESP8266)
  mb.begin(&Serial0);
  // #else
  //   mb.begin(&Serial);
  //   //mb.begin(&Serial, RXTX_PIN);  //or use RX/TX direction control pin (if required)
  //   mb.setBaudrate(9600);
  // #endif
    mb.slave(SLAVE_ID);
    mb.addHreg(REGN10);
    mb.addHreg(REGN11);
    mb.addHreg(REGN12);
    mb.addHreg(REGN13);
    mb.addHreg(REGN14);
    mb.addHreg(REGN15);
    mb.addHreg(REGN16);
    mb.addHreg(REGN17);
    mb.addHreg(REGN18);
    mb.addHreg(REGN19);
    mb.addHreg(REGN23);
    mb.Hreg(REGN10, H_data);
    mb.Hreg(REGN11, L_data);
    // mb.Hreg(REGN12, 0);
    // mb.Hreg(REGN13, 0);
    // mb.Hreg(REGN14, 0);

  // if (wifi_is_open)
  // {
  //   mb_tcp.server(TCP_PORT);		//Start Modbus IP
  //   // Add SENSOR_IREG register - Use addIreg() for analog Inputs
  //   mb_tcp.addHreg(REGN10);
  //   mb_tcp.addHreg(REGN11);
  //   mb_tcp.addHreg(REGN12);
  //   mb_tcp.addHreg(REGN13);
  //   mb_tcp.addHreg(REGN14);  
  //   mb_tcp.setTransactionId(1);
  // }
  




}